#include "simstruc.h"
#include "tmwtypes.h"
#include <libserialport.h>
#include <pthread.h>
#include <stdio.h>
#include <unistd.h> 
#include "controller/motor.h"

/* define out port/in port number & width */
int_T nInPortNums = 3;
int_T nInPortNumWidth[3] = {1,1,1};
int_T nOutPortNums = 4;
int_T nOutPortNumWidth[4] = {3, 2, 2, 1};

int_T simGetInPortNums(void) {
    return nInPortNums;
}
int_T simGetInPortNumWidth(int port) {
    return nInPortNumWidth[port];
}
int_T simGetOutPortNums(void) {
    return nOutPortNums;
}
int_T simGetOutPortNumWidth(int port) {
    return nOutPortNumWidth[port];
}

#define IN_PORTS(p)             *ssGetInputPortRealSignalPtrs(S,p)
#define Out_PORTS(p)            ssGetOutputPortRealSignal(S,p)

#define in_vd_set 			IN_PORTS(0)[0]
#define in_vq_set         	IN_PORTS(1)[0]
#define in_angle            IN_PORTS(2)[0]

#define out_pwm_duty1          Out_PORTS(0)[0]
#define out_pwm_duty2          Out_PORTS(0)[1]
#define out_pwm_duty3          Out_PORTS(0)[2]

#define out_vol_d              Out_PORTS(1)[0]
#define out_vol_q              Out_PORTS(1)[1]

#define out_vol_alpha          Out_PORTS(2)[0]
#define out_vol_beta           Out_PORTS(2)[1]

#define out_sector             Out_PORTS(3)[0]

/* need extern to bsp */
float g_motor_angle;
u16   g_pwm_duty_out[3];

void sim_sfunc_core(SimStruct *S)
{
	controller(motor(0))->foc.vol_dq_set.d = in_vd_set;
	controller(motor(0))->foc.vol_dq_set.q = in_vq_set;
	controller(motor(0))->foc.vbus_vol = 96;


	g_motor_angle = in_angle;

	motor_adc_irq_handler(motor(0));
}

void sim_sfunc_output(SimStruct *S)
{
	foc_t *foc = &controller(motor(0))->foc;
	out_pwm_duty1 = g_pwm_duty_out[0];
	out_pwm_duty2 = g_pwm_duty_out[1];
	out_pwm_duty3 = g_pwm_duty_out[2];

	
	out_vol_d = foc->vol_dq_out.d;
	out_vol_q = foc->vol_dq_out.q;

	out_vol_alpha = foc->vol_albt_out.a;
	out_vol_beta  = foc->vol_albt_out.b;

	out_sector = foc->sector;
}
